#include "HardwareAbstractionInteractionProtocolLayer.h"

TimerOne timer;

#define OUT_GROUP2        46
#define OUT_GROUP1        47
#define OUT_STROBE        24
#define OUT_KEYSELECT     25
#define OUT_BUZZER        26
#define OUT_HEATER        27
#define OUT_SPEED2        28
#define OUT_SPEED1        29
#define OUT_DATAC         30
#define OUT_DATAB         31
#define OUT_DATAA         32
#define OUT_MOTOR_RL      33
#define OUT_SOAP1         34
#define OUT_SINK          35
#define OUT_DRAIN         36
#define OUT_LOCK          37
#define IN_W2             38
#define IN_W1             39
#define IN_T2             40
#define IN_T1             41
#define IN_IN3            42
#define IN_IN2            43
#define IN_IN1            44
#define IN_IN0            45

HardwareAbstractionInteractionProtocolLayer * HardwareAbstractionInteractionProtocolLayer::instance;

HardwareAbstractionInteractionProtocolLayer::HardwareAbstractionInteractionProtocolLayer()
{
	if (!HardwareAbstractionInteractionProtocolLayer::instance) {
		HardwareAbstractionInteractionProtocolLayer::instance = this;
	}
	
	//on10CentsEntered = new EventAggregator();
	//on50CentsEntered = new EventAggregator();
	//on200CentsEntered = new EventAggregator();
	//onClear = new EventAggregator();
	//onProgramSelectPressed = new EventAggregator();
	//onStartPressed = new EventAggregator();
	//onTray1Filled = new EventAggregator();
	//onTray2Filled = new EventAggregator();
	//onIsCorrectTemp = new EventAggregator();
	//onWaterLevelAtRegisteredLevel = new EventAggregator();
	//onWaterPressureHigh = new EventAggregator();
	//onWaterPressureLow = new EventAggregator();
	//onDoorClosed = new EventAggregator();
	//onDoorOpened = new EventAggregator();

    waterPressureHigh = true;
    soapTray1Filled = soapTray2Filled = false;
    lastDoorClosed = false;
    lastWaterPressureHigh = false;
    lastValueForSoapTray1Filled = true,
    lastValueForSoapTray2Filled = true;
    startButtonListening = programButtonListening = true;
    currentProg = preProg = 0;
	isCorrectTemp = false;
	correctHeat = 0;
	statusTray1 = 0;
	statusTray2 = 0;
	waterLevel = 0;
	waterPressure = 0;
	wantedWaterLevel = 0;
	waterAlreadyAtRegisteredLevel = false;
	wantedTemp = 0;
	flushSoapTray1 = false;
	flushSoapTray2 = false;
	soapTray1Filled = false;
	soapTray2Filled = false;

	mode = KEYSELECTLOWMODE;

    moneySlotOn10 = moneySlotOn50 = moneySlotOn200 = the4thbuttonOn = reset = moneySlotListening = true;
    amtOf10sEntered = amtOf50sEntered =  amtOf200sEntered = preamtOf10sEntered = preamtOf50sEntered = preamtOf200sEntered = 0;

}

void HardwareAbstractionInteractionProtocolLayer::ignoreTheDIP(HeavyDutySuperCollidingSuperButtonDeviceController *buttons,UserInteractionGateDeviceController *door,AtomicAtomShakingDeviceController *heater,MonetaryValuedCircularMetalObjectDeviceController *money,HydrofobicAntiStainCompartiment *soap1, HydrofobicAntiStainCompartiment *soap2, StationaryWashingDevice *wash,LiquidDistributionChannelingDeviceController *water)
{
	weDontDoNoDIPButton = buttons;
	weDontDoNoDIPDoor = door;
	weDontDoNoDIPHeater = heater;
	weDontDoNoDIPMoney = money;
	weDontDoNoDIPSoap1 = soap1;
	weDontDoNoDIPSoap2 = soap2;
	weDontDoNoDIPWash = wash;
	weDontDoNoDIPWater = water;
}

HardwareAbstractionInteractionProtocolLayer::~HardwareAbstractionInteractionProtocolLayer()
{

};

void HardwareAbstractionInteractionProtocolLayer::resetPorts(void) {
	for (int i = 22; i <= 37; i++) {
		pinMode(i, OUTPUT);
	}
	for (int j = 38; j <= 45; j++) {
		pinMode(j, INPUT);
	}
	pinMode(46, OUTPUT);
	pinMode(47, OUTPUT);

	digitalWrite(OUT_GROUP2, LOW);
	digitalWrite(OUT_GROUP1, LOW);
	digitalWrite(OUT_DATAC, LOW);
	digitalWrite(OUT_DATAB, LOW);
	digitalWrite(OUT_DATAA, LOW);
	digitalWrite(OUT_STROBE, LOW);
	digitalWrite(OUT_KEYSELECT, LOW);
	digitalWrite(OUT_BUZZER, HIGH);
	digitalWrite(OUT_HEATER, HIGH);
	digitalWrite(OUT_SOAP1, LOW);
	digitalWrite(OUT_SINK, LOW);
	digitalWrite(OUT_DRAIN, LOW);
	digitalWrite(OUT_LOCK, LOW);
	digitalWrite(OUT_SPEED2, HIGH);
	digitalWrite(OUT_SPEED1, HIGH);
	digitalWrite(OUT_MOTOR_RL, LOW);

	for (int i = 16; i <= 23; i++) {
		pinMode(i, INPUT);
		//pinPullup(i, HIGH); //no pin pullup in Arduino Mega
	}
}

void globalLoop()
{
	noInterrupts();
	HardwareAbstractionInteractionProtocolLayer::instance->loopISR();
	interrupts();
}

void HardwareAbstractionInteractionProtocolLayer::initialize() {
	pinMode(56, OUTPUT);
	digitalWrite(56, HIGH);

	resetPorts();

  //TCCR1A = B01000000;
  //TCCR1B = B00001100;
  //TIMSK1 = B00000010;   /* Enables the Timer1 Compare A interrupt */
  //OCR1AH = 0xF4;
  //OCR1AL = 0x24;


	Timer1.initialize(150000);
	Timer1.attachInterrupt(globalLoop);

	
}



//water
void HardwareAbstractionInteractionProtocolLayer::openDrain()
{
    digitalWrite(OUT_DRAIN, HIGH);
}

void HardwareAbstractionInteractionProtocolLayer::closeDrain()
{
    digitalWrite(OUT_DRAIN, LOW);
}


void HardwareAbstractionInteractionProtocolLayer::openSink()
{
    digitalWrite(OUT_SINK, HIGH);
}

void HardwareAbstractionInteractionProtocolLayer::closeSink()
{
    digitalWrite(OUT_SINK, LOW);
}

void HardwareAbstractionInteractionProtocolLayer::tray1FlushSoapIntoASSWD() {
    flushSoapTray1 = true;
}

void HardwareAbstractionInteractionProtocolLayer::tray2FlushSoapIntoASSWD() {
    flushSoapTray2 = true;
}

int HardwareAbstractionInteractionProtocolLayer::getTemperature() {
	int temp = ((digitalRead(IN_T2) * 50) + ((digitalRead(IN_T2)) * (digitalRead(IN_T1)) * 50)); //ERMHABERTCH
	

	return temp;
}

bool HardwareAbstractionInteractionProtocolLayer::getIsCorrectTemp()
{
	return isCorrectTemp;
}

void HardwareAbstractionInteractionProtocolLayer::setTemperature(int temp) {
	//Serial.println("HAL:setTemperature");
	wantedTemp = temp;
}

//water
int HardwareAbstractionInteractionProtocolLayer::getWaterLevel() {
	int level1 = digitalRead(IN_W2);
	int level2 = digitalRead(IN_W1);

	if (level1 == 0) {
		if (level2 == 0) {
			return 0;
		} else {
			return 33;
		}
	} else {
		if (level2 == 0) {
			return 66;
		} else {
			return 100;
		}
	}
}


void HardwareAbstractionInteractionProtocolLayer::registerWantedWaterLevel(int level) {
	if(level==wantedWaterLevel) return;
	waterAlreadyAtRegisteredLevel= false;
	wantedWaterLevel = level;
}

//motor
int oldSpeed = 0;
void HardwareAbstractionInteractionProtocolLayer::setMotorSpeed(int speed) {
	if (speed == 0) {
		oldSpeed = speed;
		digitalWrite(OUT_SPEED1, HIGH);
		digitalWrite(OUT_SPEED2, HIGH);
	} else if (speed == 33) {
		oldSpeed = speed;
		digitalWrite(OUT_SPEED1, LOW);
		digitalWrite(OUT_SPEED2, HIGH);
	} else if (speed == 66) {
		oldSpeed = speed;
		digitalWrite(OUT_SPEED1, HIGH);
		digitalWrite(OUT_SPEED2, LOW);
	} else if (speed == 100) {
		oldSpeed = speed;
		openSink();//open sink (Hardcoded)
		digitalWrite(OUT_SPEED1, LOW);
		digitalWrite(OUT_SPEED2, LOW);
	}

	if(oldSpeed = 100 & speed!=100) //centrifuge stop
	{
		closeSink();//close sink (Hardcoded)
	}
}

void HardwareAbstractionInteractionProtocolLayer::setMotorDirection(Direction dir)
{
	digitalWrite(OUT_MOTOR_RL, dir);
}


void HardwareAbstractionInteractionProtocolLayer::lockDoor()
{
    digitalWrite(OUT_LOCK, HIGH);//led
}

void HardwareAbstractionInteractionProtocolLayer::unlockDoor()
{
    digitalWrite(OUT_LOCK, LOW);//led
}
inline void HardwareAbstractionInteractionProtocolLayer::loopISR()
{
	switch (mode)
	{
	case KEYSELECTHIGHMODE:
		mode = KEYSELECTLOWMODE;
		{
		//Serial.println("ISR high");
			//MONEY READ PHASE

			moneySlotOn10 = moneySlotOn50 = moneySlotOn200 = the4thbuttonOn = 0;

			moneySlotOn10 = digitalRead(IN_IN3);
			moneySlotOn50 = digitalRead(IN_IN2);
			moneySlotOn200 = digitalRead(IN_IN1);
			the4thbuttonOn = digitalRead(IN_IN0);

			//Serial.print("MONEY INPUT: ");
			//Serial.print(moneySlotOn10);
			//Serial.print(" ");
			//Serial.print(moneySlotOn50);
			//Serial.print(" ");
			//Serial.println(moneySlotOn200);
			//MONEY WRITE PHASE

			if (moneySlotListening) //a coin is entered at this point.
			{
				/*Serial.println("MONEY LISTENING");*/
				moneySlotListening = false;
				//REACT TO MONEY ENTERED;
				if (moneySlotOn10 && moneySlotOn50 && moneySlotOn200) //reset
				{
					//TODO: PROBLEM: assumption all buttons are down for reset at the same time. Only way is to solve with a observationtime (non-blocking-delay).
					reset = true;
				}
				else if (the4thbuttonOn)
				{
				}
				else if (moneySlotOn10)
				{
					if (amtOf10sEntered!=3)//equals 3 coins
					{
						//on10CentsEntered->execute();
						weDontDoNoDIPMoney->cents10Entered();
						amtOf10sEntered++; //BOTH A HAL COPY AND A MONEYCONTROLLER COPY OF AMOUNTOFMONEY IS KEPT. HAL COPY IS FOR LEDS.
					}
				}
				else if (moneySlotOn50)
				{
					if (amtOf50sEntered!=3)//equals 3 coins
					{
						//on50CentsEntered->execute();
						weDontDoNoDIPMoney->cents50Entered();
						amtOf50sEntered++;
					}
				}
				else if (moneySlotOn200)
				{
					if (amtOf200sEntered!=2)//equals 2 coins
					{
						//on200CentsEntered->execute();
						weDontDoNoDIPMoney->cents200Entered();
						amtOf200sEntered++;
					}
				}

			}

			if (reset)
			{
				//TODO: Should we also reset the soap tray filled values? 
				soapTray1Filled = soapTray2Filled = false;
				reset = false;
				//onClear->execute();
				weDontDoNoDIPMoney->onClear();
				amtOf10sEntered = amtOf50sEntered = amtOf200sEntered = 0;
			}
			//ASSUMING ONLY ONE COIN CAN BE ENTERED AT A TIME
			if (!(moneySlotOn10 || moneySlotOn50 || moneySlotOn200)) //start listening when buttons released.
			{
				moneySlotListening = true;
			}

			//SET LEDS ACCORDING TO AMT OF MONEY:
			//for 10:
			if(amtOf10sEntered != preamtOf10sEntered)
			{
				preamtOf10sEntered = amtOf10sEntered;
				digitalWrite(OUT_GROUP2, LOW);
				digitalWrite(OUT_GROUP1, LOW);

				digitalWrite(OUT_DATAA, amtOf10sEntered >= 1);
				digitalWrite(OUT_DATAB, amtOf10sEntered >= 2);
				digitalWrite(OUT_DATAC, amtOf10sEntered >= 3);
				digitalWrite(OUT_STROBE, HIGH);
				digitalWrite(OUT_STROBE, LOW);
			}

			//for 50:
			else if(amtOf50sEntered != preamtOf50sEntered)
			{
					Serial.println("SET MONEY 50");
				preamtOf50sEntered = amtOf50sEntered;
				digitalWrite(OUT_GROUP2, LOW);
				digitalWrite(OUT_GROUP1, HIGH);

				digitalWrite(OUT_DATAA, amtOf50sEntered >= 1);
				digitalWrite(OUT_DATAB, amtOf50sEntered >= 2);
				digitalWrite(OUT_DATAC, amtOf50sEntered >= 3);
				digitalWrite(OUT_STROBE, HIGH);
				digitalWrite(OUT_STROBE, LOW);
			}

			//for 200:
			else if(amtOf200sEntered != preamtOf200sEntered)
			{
					Serial.println("SET MONEY 200");
				preamtOf200sEntered = amtOf200sEntered;
				digitalWrite(OUT_GROUP2, HIGH);
				digitalWrite(OUT_GROUP1, LOW);

				digitalWrite(OUT_DATAA, amtOf200sEntered >= 1);
				digitalWrite(OUT_DATAB, amtOf200sEntered >= 2);
				digitalWrite(OUT_DATAC, soapTray2Filled);
				digitalWrite(OUT_STROBE, HIGH);
				digitalWrite(OUT_STROBE, LOW);
				Serial.println(soapTray2Filled);
			}
	
			//BUTTON READ PHASE
			if (programButtonListening)
			{
				programButtonListening = false;

				if (the4thbuttonOn && moneySlotOn10) //APPARENTLY ALSO CALLED WHEN ASSWD DISABLED.
				{
					currentProg++;
					if (currentProg==4)
					{
						currentProg = 1;
					}
					weDontDoNoDIPWash->onSelectProgram();
				}
			}
	
			if(startButtonListening)
			{
				startButtonListening = false;
				if (the4thbuttonOn && !moneySlotOn10) //start program
				{
					digitalWrite(OUT_BUZZER, LOW);
					reset = weDontDoNoDIPWash->onStartPressed();
					digitalWrite(OUT_BUZZER, HIGH);
         
				}
			}
			if (currentProg != preProg)
			{
				preProg = currentProg;
					Serial.println("SET PROGRAM leds");
					if (currentProg == 0)
					{
						digitalWrite(OUT_GROUP2, HIGH);
						digitalWrite(OUT_GROUP1, HIGH);
						digitalWrite(OUT_DATAA, LOW);
						digitalWrite(OUT_DATAB, LOW);
						digitalWrite(OUT_DATAC, LOW);

						digitalWrite(OUT_STROBE, HIGH);
						digitalWrite(OUT_STROBE, LOW);
					}
					else if (currentProg == 1)
					{
						digitalWrite(OUT_GROUP2, HIGH);
						digitalWrite(OUT_GROUP1, HIGH);
						digitalWrite(OUT_DATAA, HIGH);
						digitalWrite(OUT_DATAB, LOW);
						digitalWrite(OUT_DATAC, LOW);

						digitalWrite(OUT_STROBE, HIGH);
						digitalWrite(OUT_STROBE, LOW);
					}
					else if (currentProg == 2)
					{
						digitalWrite(OUT_GROUP2, HIGH);
						digitalWrite(OUT_GROUP1, HIGH);
						digitalWrite(OUT_DATAA, LOW);
						digitalWrite(OUT_DATAB, HIGH);
						digitalWrite(OUT_DATAC, LOW);

						digitalWrite(OUT_STROBE, HIGH);
						digitalWrite(OUT_STROBE, LOW);
					}
					else if (currentProg == 3)
					{
						digitalWrite(OUT_GROUP2, HIGH);
						digitalWrite(OUT_GROUP1, HIGH);
						digitalWrite(OUT_DATAA, LOW);
						digitalWrite(OUT_DATAB, LOW);
						digitalWrite(OUT_DATAC, HIGH);

						digitalWrite(OUT_STROBE, HIGH);
						digitalWrite(OUT_STROBE, LOW);
					}
			}
	
			//BUTTON DOWN
			if (!(moneySlotOn10 || the4thbuttonOn))
				programButtonListening = true;

			if(!(!moneySlotOn10 && the4thbuttonOn))
				startButtonListening = true;
	
			
			digitalWrite(OUT_KEYSELECT, LOW);
	}
	break;
	case KEYSELECTLOWMODE:
		mode = KEYSELECTHIGHMODE;
		{	
			Serial.println("");


			soapTray1Filled = flushSoapTray1 ? false :( digitalRead(IN_IN1) ? true : soapTray1Filled); //BECAUSE SWITCH DOES NOT GET SWITCHED WHEN WE FLUSH; This guarantees that when it is once true, it will always be true.
			if (flushSoapTray1) {flushSoapTray1 = false;} //it is flushed, so we don't have to flush it anymore.

			if(soapTray1Filled != lastValueForSoapTray1Filled)
			{
				lastValueForSoapTray1Filled = soapTray1Filled;
				digitalWrite(OUT_SOAP1, soapTray1Filled);
				if (soapTray1Filled)
				{
					//onTray1Filled->execute();
					weDontDoNoDIPSoap1->onTrayFilled();

				}
				else
				{
					weDontDoNoDIPSoap1->onTrayFlushed();
				}

			}
	
			
			soapTray2Filled = flushSoapTray2 ? false : (digitalRead(IN_IN2) ? true : soapTray2Filled); //BECAUSE SWITCH DOES NOT GET SWITCHED WHEN WE FLUSH;
			if (flushSoapTray2) {flushSoapTray2 = false;}

			if(soapTray2Filled != lastValueForSoapTray2Filled)
			{
				lastValueForSoapTray2Filled = soapTray2Filled;
				digitalWrite(OUT_GROUP1, LOW);
				digitalWrite(OUT_GROUP2, HIGH);

				
				digitalWrite(OUT_DATAA, amtOf200sEntered >= 1);//ALWAYS A,B AND C.
				digitalWrite(OUT_DATAB, amtOf200sEntered >= 2);//
				digitalWrite(OUT_DATAC, soapTray2Filled);
				if (soapTray2Filled)
				{
					//onTray1Filled->execute();
					weDontDoNoDIPSoap2->onTrayFilled();
				}
				else
				{
					weDontDoNoDIPSoap2->onTrayFlushed();
				}

				digitalWrite(OUT_STROBE, HIGH); //WRITE IT
				digitalWrite(OUT_STROBE, LOW);
		
			}
			int temp = getTemperature();
			/*
			if(temp==wantedTemp)
			{
				isCorrectTemp = true;
				weDontDoNoDIPHeater->onCorrectTemp();
			}
			else
			{
				isCorrectTemp = false;
			}*/
			//HEATER PHASE
			if (temp <= wantedTemp)
			{
				if(getWaterLevel()>=33) //HARDCODE CHECK. IF NOT ASSWD GOES BOOM!
				{
					digitalWrite(OUT_HEATER, LOW);
				}
			}
			else
			{
				digitalWrite(OUT_HEATER, HIGH);
			}
			/*
			//WATER READ PHASE
			waterPressureHigh = digitalRead(IN_IN0);

			if (waterPressureHigh != lastWaterPressureHigh)
			{
				lastWaterPressureHigh = waterPressureHigh;
				if (waterPressureHigh)
				{
					//onWaterPressureHigh->execute();
				} else {
					//onWaterPressureLow->execute();
				}
			}*/
			/*
			//WATER WRITE PHASE
			int currWaterLevel = getWaterLevel();
			if (currWaterLevel == wantedWaterLevel)
			{
				if (!waterAlreadyAtRegisteredLevel)
				{
					waterAlreadyAtRegisteredLevel = true;
						//onWaterLevelAtRegisteredLevel->execute();
					weDontDoNoDIPWater->onRequiredWaterLevel();
				}
			}
			else
			{
				waterAlreadyAtRegisteredLevel = false;
			}
			*/
			//waterAtRegisteredLevel = (getWaterLevel()==50); //HARDCODE
	
			//DOOR PHASE
			
			bool doorClosed;
			doorClosed = digitalRead(IN_IN3);
			if (lastDoorClosed != doorClosed)
			{
				lastDoorClosed = doorClosed;
				if (doorClosed) {
					//onDoorClosed->execute();
					weDontDoNoDIPDoor->onDoorClosed();
				} else {
					//onDoorOpened->execute();
					weDontDoNoDIPDoor->onDoorOpened();
				}
			}
	
			digitalWrite(OUT_KEYSELECT, HIGH);
			}
			break;
	}
	return;
}

//ISR(TIMER1_COMPA_vect)
//{
//	noInterrupts();
//	HardwareAbstractionInteractionProtocolLayer::instance->loopISR();
//	interrupts();
//	return;
//}